#!/usr/bin/python

import rospy
import time
import collections
import numpy as np
import random
from std_msgs.msg import Float64, Float32, Int16, String
from std_msgs.msg import Bool

from sailing_robot.sail_table import SailTable, SailData

from sailing_robot.pid_data import PID_Data
import sailing_robot.pid_control as _PID
from sailing_robot.navigation import angle_subtract


# Sheet control
WIND      = object()
SHEET_IN  = object()
SHEET_OUT = object()

# Rudder control
PID_GOAL_HEADING  = object()
PID_ANGLE_TO_WIND = object() # set an angle to the wind, 0 being going torward the wind, can be either 0/360 or -180/180
RUDDER_FULL_LEFT  = object() # the boat is going to the left
RUDDER_FULL_RIGHT = object() # the boat is going to the right

# Publishers for rudder and sailsheet control
PUB_RUDDER    = rospy.Publisher('rudder_control', Int16, queue_size=10)  # Use UInt 16 here to minimize the memory use
PUB_SAILSHEET = rospy.Publisher('sailsheet_normalized', Float32, queue_size=10)

PUB_dbg_helming = rospy.Publisher('dbg_helming_procedure', String, queue_size=10)


data = PID_Data()
rudder = rospy.get_param('rudder')
controller = _PID.PID(rudder['control']['Kp'], rudder['control']['Ki'], rudder['control']['Kd'],rudder['maxAngle'], -rudder['maxAngle'])
remote_control = False

sail_table_dict   = rospy.get_param('sailsettings/table')
sheet_out_to_jibe = rospy.get_param('sailsettings/sheet_out_to_jibe', False)
sail_table        = SailTable(sail_table_dict)
sail_data         = SailData(sail_table)

TIMEOUT      = rospy.get_param('procedure/timeout')
EXPLORE_COEF = rospy.get_param('procedure/exploration_coefficient')

##########################################################################
def update_remote_control(msg):
    remote_control = msg.data

def set_sail(sheet_control, offset=0.0):
    if sheet_control is WIND:
        sheet_normalized = sail_data.calculate_sheet_setting() + offset

        
    elif sheet_control is IN:
        sheet_normalized = 0

    elif sheet_control is OUT:
        sheet_normalized = 1

    elif sheet_control is float:
        sheet_normalized = sheet_control

    # be sure we don't publish values above 1 and below 0
    sheet_normalized = np.clip(sheet_normalized, 0, 1)
    PUB_SAILSHEET.publish(sheet_normalized)
    

def set_rudder(state, angle_to_wind=0):
    if state is PID_GOAL_HEADING:
        rawangle = -controller.update_PID(angle_subtract(data.heading, data.goal_heading))
        angle = _PID.saturation(rawangle,-rudder['maxAngle'], rudder['maxAngle'])

    elif state is PID_ANGLE_TO_WIND:
        rawangle = -controller.update_PID(angle_subtract(angle_to_wind, sail_data.wind_direction_apparent)) 
        angle = _PID.saturation(rawangle,-rudder['maxAngle'], rudder['maxAngle'])

    elif state is RUDDER_FULL_LEFT:
        angle = rudder['maxAngle']

    elif state is RUDDER_FULL_RIGHT:
        angle = -rudder['maxAngle']

    PUB_RUDDER.publish(int(angle))

##########################################################################



class ProcedureBase(object):
    def __init__(self, sailing_state, timeout=TIMEOUT):
        self.start_time = time.time()
        self.timeout = timeout
        self.sailing_state = sailing_state

    def has_failed(self):
        """
        Am I out of time?
        Am I failing?
        """
        currenttime = time.time()
        return self.EnlapsedTime() > self.timeout

    def EnlapsedTime(self):
        currenttime = time.time()
        return currenttime - self.start_time 

    def __str__(self):
        return self.__class__.__name__


class TackBasic(ProcedureBase):
    """
    Basic Tack procedure
    """
    def __init__(self, sailing_state, timeout=TIMEOUT):
        super(TackBasic, self).__init__(sailing_state, timeout) 

    def loop(self):
        set_sail(WIND)
        if self.sailing_state == "switch_to_port_tack":
            set_rudder(RUDDER_FULL_RIGHT) 
        else:
            set_rudder(RUDDER_FULL_LEFT)



class JibeBasic(ProcedureBase):
    """
    Basic Jibe procedure
    """
    def __init__(self, sailing_state, timeout=TIMEOUT):
        super(JibeBasic, self).__init__(sailing_state, timeout) 

    def loop(self):
        # sheet out a bit more than what is given by the look up table
        set_sail(WIND, offset=+0.2)
        if self.sailing_state == "switch_to_port_tack":
            set_rudder(RUDDER_FULL_LEFT) 
        else:
            set_rudder(RUDDER_FULL_RIGHT)


class TackSheetOut(ProcedureBase):
    """
    Tack procedure where we sheet out a bit

    When sheeted in completely the jib has too much power and tacking becomes impossible
    in some strong conditions. Hence sheeting out is needed, however if the sails are out
    too much the boat will not have enough power to tack 
    """
    def __init__(self, sailing_state, timeout=TIMEOUT):
        super(TackSheetOut, self).__init__(sailing_state, timeout) 

    def loop(self):
        # sheet out a bit more than what is given by the look up table
        set_sail(WIND, offset=+0.2)
        if self.sailing_state == "switch_to_port_tack":
            set_rudder(RUDDER_FULL_RIGHT) 
        else:
            set_rudder(RUDDER_FULL_LEFT)



class Tack_IncreaseAngleToWind(ProcedureBase):
    """
    More advance Tack procedure, building speed for 5s by going less upwind
    """
    def __init__(self, sailing_state, timeout=TIMEOUT):
        super(Tack_IncreaseAngleToWind, self).__init__(sailing_state, timeout) 
        self.beating_angle = 80

    def loop(self):
        set_sail(WIND)
        if self.EnlapsedTime() < 4:
            if self.sailing_state == "switch_to_port_tack":
                set_rudder(PID_ANGLE_TO_WIND, angle_to_wind = self.beating_angle)
            else:
                set_rudder(PID_ANGLE_TO_WIND, angle_to_wind = 360-self.beating_angle)  
        else:
            if self.sailing_state == "switch_to_port_tack":
                set_rudder(RUDDER_FULL_RIGHT) 
            else:
                set_rudder(RUDDER_FULL_LEFT)



##########################################################################

class ProcedureHandle():
    """
        Class to handle a list of procedure and the priority based on weights
        after each tack attempt weight based on time taken by the procedure
        are given to each procedure for the future.
    """
    def __init__(self, ProcedureList):

        # Initialisation of the procedure list with initial weights, integers with increment of 1
        self.ProcedureList = [ {"Procedure": Procedure, 
                                "TimeList": collections.deque(maxlen = 10),
                                "InitPos": i} for i,Procedure in enumerate(ProcedureList) ]

        self.currentProcedureId = 0
        self.currentProcedure = None

    def ProcedureInProgress(self):
        return (self.currentProcedure != None)

    def FirstProcedure(self):
        self.OrderList()
        self.currentProcedureId = 0

    def NextProcedure(self):
        self.currentProcedureId = (self.currentProcedureId + 1) % len(self.ProcedureList)

    def OrderList(self):
        def get_weight(x):
            if x['TimeList']:
                return np.mean(x['TimeList'])
            else:
                if random.random() < (EXPLORE_COEF / sum([ not x['TimeList'] for x in self.ProcedureList])):
                    rospy.logwarn("Random procedure picked")
                    # Add some randomness in choice for untested procedures
                    return 0.1*random.random()
                else:
                    # Just to keep the order given at first if the procedure was not tested yet
                    return TIMEOUT + x['InitPos']*0.01*TIMEOUT

        self.ProcedureList = sorted(self.ProcedureList, key=get_weight)
        # rospy.logwarn(str(self.ProcedureList))

    def MarkSuccess(self):
        if not remote_control:
            PUB_dbg_helming.publish("success " + str(self.currentProcedure))
            self.ProcedureList[self.currentProcedureId]['TimeList'].append(self.currentProcedure.EnlapsedTime())
        self.currentProcedure = None

    def MarkFailure(self):
        if not remote_control:
            PUB_dbg_helming.publish("fail " + str(self.currentProcedure))
            # In case of failure the weight given is 1.5 times the timeout
            self.ProcedureList[self.currentProcedureId]['TimeList'].append(1.5*TIMEOUT)
        self.currentProcedure = None

    def StartProcedure(self, sailing_state):
        self.currentProcedure = self.ProcedureList[self.currentProcedureId]['Procedure'](sailing_state)
        PUB_dbg_helming.publish("start " + str(self.currentProcedure))


##########################################################################



class Helming():
    def __init__(self):
        rospy.init_node('helming', anonymous=True)
        self.rate = rospy.Rate(rospy.get_param("config/rate"))

        # Initialisation of the procedure list
        if rospy.get_param('procedure/jibe_to_turn'):
            procedureList = [JibeBasic, TackBasic, TackSheetOut, Tack_IncreaseAngleToWind]
        else:
            procedureList = [TackBasic, TackSheetOut, Tack_IncreaseAngleToWind, JibeBasic]


        self.Proc = ProcedureHandle(procedureList)
        self.Runner()

    def Runner(self):
        while not rospy.is_shutdown():
            if data.sailing_state == 'normal':
                if self.Proc.ProcedureInProgress():
                    # ending a procedure because it is finished according to the highlevel
                    rospy.logwarn("Procedure success "+ str(self.Proc.currentProcedure)+
                                  " in "+ '{:.2f}'.format(self.Proc.currentProcedure.EnlapsedTime()) + "s")
                    self.Proc.MarkSuccess()

                set_rudder(PID_GOAL_HEADING)
                set_sail(WIND)
            else:
                # Continuing a procedure
                self.runProcedure()

            self.rate.sleep()


    def runProcedure(self):
        if (not self.Proc.ProcedureInProgress()) or (data.sailing_state != self.Proc.currentProcedure.sailing_state):
            # no procedure have been started (=we just decided to swich tack)
            self.Proc.FirstProcedure()
            self.Proc.StartProcedure(data.sailing_state)
            rospy.logwarn("Run procedure     " + str(self.Proc.currentProcedure))

        elif self.Proc.currentProcedure.has_failed():
            rospy.logwarn("Procedure failed  " + str(self.Proc.currentProcedure))

            self.Proc.MarkFailure()
            # if time out we start the next procedure in the list
            self.Proc.NextProcedure()
            self.Proc.StartProcedure(data.sailing_state)
            rospy.logwarn("Run procedure     " + str(self.Proc.currentProcedure))

        # we advance to the next timestep 
        self.Proc.currentProcedure.loop()




if __name__ == '__main__':
    try:
        rospy.Subscriber('wind_direction_apparent', Float64, sail_data.update_wind)
        rospy.Subscriber('goal_heading', Float32, data.update_goal_heading)
        rospy.Subscriber('heading', Float32, data.update_heading)
        rospy.Subscriber('sailing_state', String, data.update_sailing_state)
        rospy.Subscriber('tack_rudder', Float32, data.update_tack_rudder)
        rospy.Subscriber('remote_control', Bool, update_remote_control)
        Helming()
    except rospy.ROSInterruptException:
        pass

########################################################################



